#pragma once
#ifndef CO_LOCATION_COMPUTER_HPP_
#define CO_LOCATION_COMPUTER_HPP_

#include "IMM_UKF.hpp"
#include "Airplane.hpp"
#include "DDXTJC.hpp"

// #include "ROSLinker.hpp"

// #include <iostream>
// #include <stdlib.h>
// #include <string>
// #include <unordered_map>
// #include <algorithm>
// #include <random>
#include <memory>

// ukf的P0均设定为对角线为INITIAL_P_SIGMA的单位阵
#define INITIAL_P_SIGMA 0.1

extern double data3[];//pixel noise cov

extern Eigen::Map<Eigen::Matrix<double, 2, 2>> temp_r;
extern const Eigen::MatrixXd default_Ri;

// 整个算法每收到一次数据，运行一次
class Co_Location_Computer{
public:
	Co_Location_Computer(){
	};

	~Co_Location_Computer(){
	};

	//// 上层调用接口
	// 创建新的飞机
	// 取数据时不允许对飞机数量执行增减
	bool CreateNewAirplane(Eigen::Vector3d t_g,
            		   Rotation_Angles Pod_Rotation_Angles,
					   uint8_t AirplaneId,
                       Gimbal_Camera Gimbal_camera_parameter,
					   Eigen::MatrixXd Ri = default_Ri);

    bool DeleteAirplane(uint8_t AirplaneId);

	bool ChangeAirplanePayload(uint8_t AirplaneId, uint8_t payload_type, uint8_t detail_type, std::vector<double> Datas);

    //查询已创建的飞机ID
	std::vector<uint8_t> GetAllAirplanesId();

	std::vector<uint8_t> GetAllTargetsId();

	// 创建新目标 第一步
	// 同时需要创建相关算法对象
	// 返回创建失败的飞机id
	bool CreateNewTarget(std::vector<uint8_t> &failCreateTarget_Airplane, uint8_t TargetId, uint64_t Time_measure);

	bool TargetIsCreated(uint8_t TargetId);

	bool AirplaneIsCreated(uint8_t AirplaneId);

	// 删除已有目标
	// 若所有飞机均完全丢失某个目标的信息，则删除该目标所对应的所有数据及其算法对象
	// 亦可供用户直接调用
	// 返回删除失败信息
	bool DeleteTarget(std::vector<uint8_t> &failDeleteTarget_Airplane, uint8_t TargetId);

	// 
	// 接收数据 第二步
	// 每次执行均需要先更新TargetLostInfo_Map_，并根据是否丢失目标决定是否重新执行初始化
	// 返回值： 0更新失败   1需运行S2   2需运行S3
	uint8_t UpdateAirplaneState(Eigen::Vector3d t_n, Eigen::Vector3d t_c,
                             Rotation_Angles Gimbal_Rotation_Angles, Rotation_Angles Body_Rotation_Angles,
                             uint8_t TargetId, uint8_t AirplaneId,
                             Picture_Location PictureLocation, uint64_t Time_measure);

	// 输出数据 目标id下的数据
	bool GetEstimate_Results_(std::vector<Filtered_Target_State> &Estimate_Results, uint8_t TargetId);

	// 时间判断

	// 计算更新(所有飞机，所有目标) 第三步
	// 若RunUKF为真，运行S3，为假则运行S2
	 bool UpdateTargetState(uint8_t TargetId);

	//// bool UpdateTargetState(bool RunUKF, uint64_t time_now, uint64_t time_delta, uint8_t TargetId);

	// 若所有飞机对该目标的量测间隔均超过该值，则认为目标彻底丢失
	void SetTargetLostInfoTargetLostTimeMax(uint8_t TargetId, uint64_t TargetLostTimeMax);

	// 若单个飞机对该目标的量测间隔超过该值，则认为当前Id的飞机已丢失目标
	void SetTargetLostInfoTargetLostTimeMaxForOneAirplane(uint8_t AirplaneId, uint8_t TargetId, uint64_t TargetLostTimeMax);

	//// S2结果已获得 已被综合入TargetLostInfo_Map_中
	//// 	bool Isinitalized(uint8_t TargetId);

private:
	// 每收到一次数据，运行一次
	void UpdateTargetLostInfo_Map_(uint8_t TargetId, uint64_t Time_measure);

	// 初始化 (调用S2，传递结果至S3)
	// 需要计算测量时间的均值，并作为基准时间输入S3
//	bool Initialization(uint8_t TargetId);

	// S3更新目标状态
	// 需要取用所有飞机中包含的关于该目标的所有测量数据
	// 同时，测量数据需要根据测量时间进行分组,生成多个MeasureMents_In_Period并存入IMM_UKF
	// delta_t: 认为属于同一时间范围的最大时间间隔
	bool UKF_Process(uint8_t TargetId, uint64_t delta_t);

	/**
	 * @brief 生成MeasureMents_In_Period
	 * 			从飞机中取出测量结果
	 */
	MeasureMents_In_Period Generate_UKF_Input(std::vector<Measurement_Pair> &Measurements, std::vector<Eigen::MatrixXd> &Ri);

	/**
	 * @brief 根据该时间段内的量测数据数量，和对应飞机生成合并的矩阵R
	 * 		根据测量时间分组结果，将飞机的Ri融合
	 */
	Eigen::MatrixXd Generte_R_merge_(std::vector<Eigen::MatrixXd> &Ri, uint8_t PeriodNum);

	bool CreateNewTarget(uint8_t TargetId, uint64_t Time_measure);

	// // 若ukf发散或目标丢失，则需重新初始化
	// std::unordered_map<uint8_t,bool> is_initalized_;//目标id， 已执行S2：true 

	// 飞机Id，飞机
	std::unordered_map<uint8_t,Airplane> airplanes_;//机群容器，每个飞机需要拥有一个id号

	//uint8_t CurAirplaneNum = 0;//当前已创建的用于监测目标位置的无人机数量

	std::vector<uint8_t> TargetsId_;//每个目标需要固定一个id号

	//uint8_t CurTargetNum = 0;//当前正在追踪的目标数量

	// 若所有飞机均完全丢失某个目标，则删除所有飞机下的对应目标数据，以及删除对应目标的所有算法对象
	// 目标id，所有飞机均丢失该目标的计数,初始化等信息
	std::unordered_map<uint8_t,TargetLostInfo> TargetLostInfo_Map_;

	// 测量时间相差超过10ms，则认为两张图片在不同的时间测量，
	// 则其ukf计算需要分割成两步串联（异步量测）
	uint64_t Max_delta_t_ukf_ = 10;

	// 允许同时跑多个算法，但每个算法需要固定一个目标id号
	std::unordered_map<uint8_t,DDXTJC> ukf_initalizer_; //S2 目标id，对应的S2

	std::unordered_map<uint8_t,IMM_UKF> imm_ukf_filters_;//S3 目标id，对应的IMM-UKF

	uint8_t MaxSavedEstimationNum = 6;
	// 滤波后的目标状态量结果: 每次计算结果保存于此，最多保存5个结果
	std::unordered_map<uint8_t,std::vector<Filtered_Target_State>> Estimate_Results_;

	// //ROS数据接收发送端
	// ROSLinker ClientROSLinker_;

	// TargetID, true:S3/false:S2
	std::unordered_map<uint8_t,bool> StepChooseMap_;

	// 数据保护  当前无需特别保护处理， 若计算过程与增删发生冲突，需考虑此问题（迭代计算按特定频率运行时）
	std::unordered_map<uint8_t,bool> ForbidAddOrDeleteTarget_map_;

	std::unordered_map<uint8_t,bool> ForbidAddOrDeleteAirplane_map_;

	// 删除目标失败的飞机

	// 创建目标失败的飞机

	// 
};

#endif


